/*
 * NxMSFollowLeader.java
 *
 * Created on 7 de noviembre de 2007, 11:19 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package nx.behaviors;
import nx.utils.*;
import javaclient3.*;
import javaclient3.structures.*;

/**
 * Class implementing a Motor Schema (MS) for following robots or other devices.
 * The detection is made by NxPSDetectLeader.
 * 
 * @author Alejandro Pustowka, Jonathan Hernandez
 */
public class NxMSFollowLeader extends NxBehavior{
    
    float maxDist = 2.0f;
    float minDistToLeader = 0.4f;
    Position2DInterface myPosition;
    NxBehavior leaderPosition;
    
    
    /**
     * Creates a new instance of NxMSFollowLeader
     * @param name Name of this behavior.
     * @param myPosition The position of this robot.
     * @param leaderPosition Perception Schema (PS) that detects the leader.
     * @param maxDist Distance from where the robot's speed will be the maximum.
     * @param minDistToLeader At this distance to the leader the robot will stop.
     */
    public NxMSFollowLeader(String name,Position2DInterface myPosition, NxBehavior leaderPosition,float maxDist, float minDistToLeader) {
        super(name);
        this.myPosition = myPosition;
        this.leaderPosition = leaderPosition;
        this.maxDist = maxDist;
        this.minDistToLeader = minDistToLeader;
    }

    /**
     * Output of this behavior. A vector that joins Leader and Follower.
     * @return Vector Output.
     */
    public Vector2D value() {
        
        PlayerPose myPose = myPosition.getData().getPos();//.read(RealPosition2d.READ_MODE_NO_WAIT);
        
        
        Vector2D myPos=new Vector2D(myPose.getPx(),myPose.getPy(),Vector2D.MODE_XY);
        
        //Get leader position from Perceptual NxBehavior
        Vector2D leaderPos = (Vector2D)leaderPosition.value();
        
        Vector2D atractor = leaderPos.sub(myPos);
        
        //if robot is beyond of minimal distance to leader
        if(atractor.getMag() > minDistToLeader)
            //If it is far away, max vel
            if(atractor.getMag() > maxDist){
                atractor = atractor.normalize();
                System.out.println("aqui");
                atractor.print(Vector2D.MODE_MA);
                return atractor;
            }
            else
            {
                //if it is near, scale vel according to distance
                System.out.println("aca");
                atractor = atractor.scale(1/maxDist);
                atractor.print(Vector2D.MODE_MA);
                return atractor;
            }
        else
            //robot is in the required position, it is inside the sphere of proximity
            return new Vector2D(0,0,Vector2D.MODE_XY);

    }
    
}
